#include "core/matrix.hpp"

Matrix::Matrix(float* M)
{
  memcpy(&m[0][0], M, sizeof(m));
  assert(!hasnans());
}

Matrix::Matrix(float c00, float c01, float c02, float c03,
	       float c10, float c11, float c12, float c13,
	       float c20, float c21, float c22, float c23,
	       float c30, float c31, float c32, float c33)
{
  const float tmp[4][4] = {c00,c01,c02,c03,
			   c10,c11,c12,c13,
			   c20,c21,c22,c23,
			   c30,c31,c32,c33};
  memcpy(&m[0][0], &tmp[0][0], sizeof(m));
  assert(!hasnans());
}

Matrix::Matrix(const Matrix& o)
{
  memcpy(&m[0][0], &o.m[0][0], sizeof(m));
  assert(!hasnans());
}

Matrix Matrix::transpose() const
{
  return Matrix(m[0][0],m[1][0],m[2][0],m[3][0],
		m[0][1],m[1][1],m[2][1],m[3][1],
		m[0][2],m[1][2],m[2][2],m[3][2],
		m[0][3],m[1][3],m[2][3],m[3][3]);
}

//BORROWED FROM PBRT
Matrix Matrix::inverse() const
{
  int indxc[4], indxr[4];
  int ipiv[4] = { 0, 0, 0, 0 };
  float minv[4][4];
  memcpy(minv, m, 4*4*sizeof(float));
  for (int i = 0; i < 4; i++) {
    int irow = -1, icol = -1;
    float big = 0.;
    // Choose pivot
    for (int j = 0; j < 4; j++) {
      if (ipiv[j] != 1) {
	for (int k = 0; k < 4; k++) {
	  if (ipiv[k] == 0) {
	    if (fabsf(minv[j][k]) >= big) {
	      big = float(fabsf(minv[j][k]));
	      irow = j;
	      icol = k;
	    }
	  }
	  else if (ipiv[k] > 1)
	    error("Singular matrix in MatrixInvert");
	}
      }
    }
    ++ipiv[icol];
    // Swap rows _irow_ and _icol_ for pivot
    if (irow != icol) {
      for (int k = 0; k < 4; ++k)
	swap(minv[irow][k], minv[icol][k]);
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (minv[icol][icol] == 0.)
      error("Singular matrix in MatrixInvert");
    
    // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
    float pivinv = 1.f / minv[icol][icol];
    minv[icol][icol] = 1.f;
    for (int j = 0; j < 4; j++)
      minv[icol][j] *= pivinv;
    
    // Subtract this row from others to zero out their columns
    for (int j = 0; j < 4; j++) {
      if (j != icol) {
	float save = minv[j][icol];
	minv[j][icol] = 0;
	for (int k = 0; k < 4; k++)
	  minv[j][k] -= minv[icol][k]*save;
      }
    }
  }
  // Swap columns to reflect permutation
  for (int j = 3; j >= 0; j--) {
    if (indxr[j] != indxc[j]) {
      for (int k = 0; k < 4; k++)
	swap(minv[k][indxr[j]], minv[k][indxc[j]]);
    }
  }
  return Matrix(&minv[0][0]);
}

